import cv2 as cv
import numpy as np
import scipy.spatial as spt
from matplotlib import pyplot as plt

# 点集中上半部分最左边的点
def find_start_point(points: list) -> int:  # return index of point
    points_backup = np.array(points)
    points = np.array(points)
    points = points[points[:, 1].argsort()] # 按照y进行排序
    points = points[:(len(points)//3)] # 取上半边
    index = np.where(points_backup == np.min(points[:, 0], axis=0))   # 最左边的点

    return index[0][0]

# 返回半径方框的点集
def points_square(point: list, radius: int) -> list:
    add_points = []
    start_point = [radius, 0]

    while (start_point[1] > -radius):
        add_points.append([start_point[0] + point[0], start_point[1] + point[1]])  # [start_point[0] + point[0], start_point[1] + point[1]]
        start_point[1] -= 1

    while(start_point[0] > -radius):
        add_points.append([start_point[0] + point[0], start_point[1] + point[1]])
        start_point[0] -= 1

    while (start_point[1] < radius):
        add_points.append([start_point[0] + point[0], start_point[1] + point[1]])
        start_point[1] += 1

    while (start_point[0] < radius):
        add_points.append([start_point[0] + point[0], start_point[1] + point[1]])
        start_point[0] += 1

    while (start_point[1] > 0):
        add_points.append([start_point[0] + point[0], start_point[1] + point[1]])
        start_point[1] -= 1

    return add_points

# 顺时针周围8个点
# def points_around(point: list) -> list:
#     # point = list(point)[0]
#     return [[point[0]-1, point[1]-1],
#             [point[0], point[1]-1],
#             [point[0]+1, point[1]-1],
#             [point[0]+1, point[1]],
#             [point[0]+1, point[1]+1],
#             [point[0], point[1]+1],
#             [point[0]-1, point[1]+1],
#             [point[0]-1, point[1]]]
# 查找半径范围内的点,求交集
def points_around(point: list, radius: int) -> list:
    points_list = []
    i = 1

    for i in range(i, radius+1):
        points_list += points_square(point, i)

    return points_list

# 两点集求交, 返回第一个点
def list_intersection(arounds: list, points: list) -> list:
    for i in arounds:
        if (i in points):
            return i

# 点集连线问题主函数
def write_sequence(points: list, Search_radius: int) -> list:
    points_backup = points.copy()
    finders = [] # 所有线的列表

    while len(points_backup) > 3:  # 直到点队列为空
        start = points_backup[find_start_center(points_backup)[0]]    # 点集中上半部分最左边的点
        # start = points_backup[find_start_point(points_backup)]    # 提升200ms
        inter = start   # 当前查找到的点
        counter = 0
        finder = [] # 一根线中的点
        while (len(points_backup) != 0):
            # arounds = points_around(inter, Search_radius)   # 查找半径范围内的点,求交集
            # inter = list_intersection(arounds, points_backup)  # 两点集求交, 返回第一个点
            if (counter == 0):    # 从队列中删除start点并添加到finder查找队列中
                finder.append(start)
                points_backup.remove(start)
            kt = spt.KDTree(data=points_backup, leafsize=Search_radius)  # 用于快速查找的KDTree类
            d, x = kt.query(np.array(inter))  # 返回最近邻点的距离d和在数组中的顺序x
            inter = points_backup[x]
            if (d > 2): # 不存在临近点
                # if (counter < 1):   # 说明是一个单独的像素, 直接删除
                #     points_backup.remove(start)
                break
            finder.append(inter)    # 将当前查找到的点从队列中删除并添加到finder查找队列中
            points_backup.remove(inter)
            counter += 1
        if (counter > 1): # 当前线段查找完毕添加到finders
            finders.append(finder)

    # print("Num of vactors: ", len(finders))
    return finders

# 方框法求点集中心
def points_center(points: list) -> list:
    points = np.array(points)
    center = cv.minAreaRect(points)[0]
    return [int(center[0] // 1), int(center[1] // 1)]

# 求距离排序直接加和加快速度
def find_start_center(points: list) -> list:  # return index of point
    points_backup = points.copy()
    points_backup = np.sum(np.array(points_backup), axis=1)
    points_backup = points_backup.argsort()   # 按照距离进行排序
    return points_backup

# 线段排序
def lines_sort(lines: list) -> list:
    centers = []
    for line in lines:
        center = points_center(line)    # 方框法求点集中心
        centers.append(center)
    index = find_start_center(centers)  # 求与原点距离排序直接加和加快速度
    lines = np.array(lines, dtype=list)[index].tolist() # lines按照index排序

    return lines

# 点集转换为绝对坐标
def relative_position_repair(lines: list, contour: list) -> list:
    for line in lines:
        for point in line:
            point[0] += contour[0]
            point[1] += contour[1]
    return lines

if __name__ == '__main__':
    points = np.loadtxt("D:/Miniconda3/envs/pyQtApp_00/Data/grbl_Writter/results/rua1.txt", dtype=int, delimiter=',').tolist()

    show = np.zeros((300, 300, 3), np.uint8)
    # 打点
    for point in points:
        cv.circle(show, point, 1, (255, 255, 255))
    plt.imshow(show)
    plt.show()

    points = write_sequence(points)
    points = points_sort(points)

    show = np.zeros((300, 300, 3), np.uint8)
    # 打点
    for i, point in enumerate(points):
        for po in point:
            cv.circle(show, po, 1, (255 - i * 9, 255 - i * 9, 255 - i * 9))
    plt.imshow(show)
    plt.show()

    print(len(points))

    cv.waitKey(0)  # 等待用户操作,里面等待参数是毫秒,我们填写0,代表是永远,等待用户操作
    cv.destroyAllWindows()  # 销毁所有窗口